/*
 * Copyright (C) 2017
 *
 * Brick Yang <printfxxx@163.com>
 *
 * This program is free software. You can redistribute it and/or
 * modify it as you like.
 */

/**
 * @file	qbman.c
 * @brief	qbman driver
 */

#include <linux/init.h>
#include <linux/slab.h>
#include <linux/kernel.h>
#include <linux/module.h>

#include <asm/io.h>

#include <mtrace.h>

#include "qbman.h"

#define QMAN_REV_4000	0x04000000
#define QMAN_REV_4100	0x04010000
#define QMAN_REV_4101	0x04010001
#define QMAN_REV_MASK	0xffff0000

#define QB_VALID_BIT	((u32)0x80)
#define QB_RT_BIT	((u32)0x100)

/* CINH register offsets */
#define SWP_CINH_CR		0x600
#define SWP_CINH_RR(n)		(0x700 + ((u32)(n) << 6))
#define SWP_CINH_EQCR_PI	0x800
#define SWP_CINH_EQCR_CI	0x840
#define SWP_CINH_CR_RT		0x900
#define SWP_CINH_DQRR_PI	0xa00
#define SWP_CINH_DQRR_CI	0xa40
#define SWP_CINH_SDQCR		0xb00
#define SWP_CINH_RCR_PI		0xc00
#define SWP_CINH_RCR_CI		0xc40
#define SWP_CINH_CFG		0xd00
#define SWP_CINH_ISR		0xe00
#define SWP_CINH_IER		0xe40
#define SWP_CINH_ISDR		0xe80
#define SWP_CINH_IIR		0xec0

/* CENA register offsets */
#define SWP_CENA_EQCR(n)		(0x000 + ((u32)(n) << 6))
#define SWP_CENA_EQCR_IDX(o)		(((u32)(o) - 0x000) >> 6)
#define SWP_CENA_DQRR(n)		(0x200 + ((u32)(n) << 6))
#define SWP_CENA_DQRR_IDX(o)		(((u32)(o) - 0x200) >> 6)
#define SWP_CENA_RCR(n)			(0x400 + ((u32)(n) << 6))
#define SWP_CENA_RCR_IDX(o)		(((u32)(o) - 0x400) >> 6)
#define SWP_CENA_CR			0x600
#define SWP_CENA_RR(n)			(0x700 + ((u32)(n) << 6))
#define SWP_CENA_EQCR_CI		0x840
#define SWP_CENA_DQRR_PI		0xa00
#define SWP_CENA_RCR_CI			0xc40
#define SWP_CENA_EQCR_CI_MEMBACK	0x1840

/* CENA register offsets in memory-backed mode */
#define SWP_CENA_DQRR_MEM(n)		(0x800 + ((u32)(n) << 6))
#define SWP_CENA_DQRR_MEM_IDX(o)	(((u32)(o) - 0x800) >> 6)
#define SWP_CENA_RCR_MEM(n)		(0x1400 + ((u32)(n) << 6))
#define SWP_CENA_RCR_MEM_IDX(o)		(((u32)(o) - 0x1400) >> 6)
#define SWP_CENA_CR_MEM			0x1600
#define SWP_CENA_RR_MEM			0x1680
#define SWP_CENA_VDQCR_MEM		0x1780

/* SDQCR attribute codes */
#define SWP_SDQCR_FC_SHIFT	29
#define SWP_SDQCR_FC_MASK	0x1
#define SWP_SDQCR_DCT_SHIFT	24
#define SWP_SDQCR_DCT_MASK	0x3
#define SWP_SDQCR_TOK_SHIFT	16
#define SWP_SDQCR_TOK_MASK	0xff
#define SWP_SDQCR_SRC_SHIFT	0
#define SWP_SDQCR_SRC_MASK	0xffff

/* opaque token for static dequeues */
#define QMAN_SDQCR_TOKEN	0xbb

#define dccvac(p)	{ asm volatile("dc cvac, %0;" : : "r" (p) : "memory"); }
#define dcivac(p)	{ asm volatile("dc ivac, %0;" : : "r" (p) : "memory"); }
#define dczva(p)	{ asm volatile("dc zva,  %0;" : : "r" (p) : "memory"); }

enum qbman_sdqcr_dct {
	qbman_sdqcr_dct_null = 0,
	qbman_sdqcr_dct_prio_ics,
	qbman_sdqcr_dct_active_ics,
	qbman_sdqcr_dct_active
};
enum qbman_sdqcr_fc {
	qbman_sdqcr_fc_one = 0,
	qbman_sdqcr_fc_up_to_3 = 1
};

/* Portal Access */

static inline u32 qbman_cinh_read(struct qbman_swp *s, u32 offset)
{
	__le32 le32;

	le32 = __raw_readl(s->addr_cinh + offset);
	return le32_to_cpu(le32);
}

static inline void qbman_cinh_write(struct qbman_swp *s, u32 offset,
				    u32 val)
{
	__le32 le32;

	le32 = cpu_to_le32(val);
	__raw_writel(le32, s->addr_cinh + offset);
}

static inline void __iomem *qbman_cinh_base(struct qbman_swp *s, u32 offset)
{
	BUG_ON(!IS_ALIGNED(offset, SMP_CACHE_BYTES));
	return s->addr_cinh + offset;
}

static inline u32 qbman_cena_read(struct qbman_swp *s, u32 offset)
{
	__le32 le32;

	le32 = __raw_readl(s->addr_cena + offset);
	return le32_to_cpu(le32);
}

static inline void qbman_cena_write(struct qbman_swp *s, u32 offset,
				    u32 val)
{
	__le32 le32;

	le32 = cpu_to_le32(val);
	__raw_writel(le32, s->addr_cena + offset);
}

static inline void __iomem *qbman_cena_base(struct qbman_swp *s, u32 offset)
{
	BUG_ON(!IS_ALIGNED(offset, SMP_CACHE_BYTES));
	return s->addr_cena + offset;
}

static inline u8 qm_cyc_diff(u8 mask, u8 left, u8 right)
{
	return (right - left) & mask;
}

#define SWP_CFG_DQRR_MF_SHIFT	20
#define SWP_CFG_EST_SHIFT	16
#define SWP_CFG_CPBS_SHIFT	15
#define SWP_CFG_WN_SHIFT	14
#define SWP_CFG_RPM_SHIFT	12
#define SWP_CFG_DCM_SHIFT	10
#define SWP_CFG_EPM_SHIFT	8
#define SWP_CFG_VPM_SHIFT	7
#define SWP_CFG_CPM_SHIFT	6
#define SWP_CFG_SD_SHIFT	5
#define SWP_CFG_SP_SHIFT	4
#define SWP_CFG_SE_SHIFT	3
#define SWP_CFG_DP_SHIFT	2
#define SWP_CFG_DE_SHIFT	1
#define SWP_CFG_EP_SHIFT	0

static inline u32 qbman_set_swp_cfg(u8 max_fill, u8 est, u8 cpbs, u8 wn,
				    u8 rpm, u8 dcm, u8 epm, u8 vpm, u8 cpm,
				    u8 sd, u8 sp, u8 se, u8 dp, u8 de, u8 ep)
{
	return max_fill << SWP_CFG_DQRR_MF_SHIFT |
	       est << SWP_CFG_EST_SHIFT |
	       cpbs << SWP_CFG_CPBS_SHIFT |
	       wn << SWP_CFG_WN_SHIFT |
	       rpm << SWP_CFG_RPM_SHIFT |
	       dcm << SWP_CFG_DCM_SHIFT |
	       epm << SWP_CFG_EPM_SHIFT |
	       vpm << SWP_CFG_VPM_SHIFT |
	       cpm << SWP_CFG_CPM_SHIFT |
	       sd << SWP_CFG_SD_SHIFT |
	       sp << SWP_CFG_SP_SHIFT |
	       se << SWP_CFG_SE_SHIFT |
	       dp << SWP_CFG_DP_SHIFT |
	       de << SWP_CFG_DE_SHIFT |
	       ep << SWP_CFG_EP_SHIFT;
}

struct qbman_swp *qbman_swp_init(const struct qbman_swp_desc *d)
{
	u8 i, n;
	u32 reg;
	bool est, de, rcr_vb, eqcr_vb, memback;
	struct qbman_swp *s;

	if (!(s = kmalloc(sizeof(*s), GFP_KERNEL)) || MTRACE_KMEM_ADD(s)) {
		goto err;
	}
	s->desc = d;
	s->sdq = 0;
	s->sdq |= qbman_sdqcr_dct_prio_ics << SWP_SDQCR_DCT_SHIFT;
	s->sdq |= qbman_sdqcr_fc_up_to_3 << SWP_SDQCR_FC_SHIFT;
	s->sdq |= QMAN_SDQCR_TOKEN << SWP_SDQCR_TOK_SHIFT;

	if ((s->desc->qman_version & QMAN_REV_MASK) < QMAN_REV_4100) {
		s->dqrr.pi_ci_mask = 7;
	} else {
		s->dqrr.pi_ci_mask = 0xf;
	}

	s->addr_cena = d->cena_bar;
	s->addr_cinh = d->cinh_bar;

	qbman_cinh_write(s, SWP_CINH_SDQCR, 0);
	qbman_cinh_write(s, SWP_CINH_IER, 0);
	qbman_cinh_write(s, SWP_CINH_ISDR, 0xffffffff);
	qbman_cinh_write(s, SWP_CINH_IIR, 1);
	qbman_cinh_write(s, SWP_CINH_ISR, 0xffffffff);

	est = s->desc->flags & SWP_F_EST ? true : false;
	de = (s->desc->flags & SWP_F_DE) ? true : false;
	rcr_vb = s->desc->flags & SWP_F_RCR_VB ? true : false;
	eqcr_vb = s->desc->flags & SWP_F_EQCR_VB ? true : false;
	memback = s->desc->flags & SWP_F_MEMBACK ? true : false;
	if (memback) {
		if (rcr_vb) {
			pr_err("qbman: SWP_F_MEMBACK conflict with SWP_F_RCR_VB\n");
			goto err;
		}
		if (eqcr_vb) {
			pr_err("qbman: SWP_F_MEMBACK conflict with SWP_F_EQCR_VB\n");
			goto err;
		}
	}
	reg = qbman_set_swp_cfg((s->dqrr.pi_ci_mask >> 1) + 1,	/* DQRR max fill */
				est ? 4 : 0,			/* EQCR_CI stashing threshold */
				memback ? 1 : 0,		/* Cacheable Portal memory Backing Store mode */
				0,				/* Writes cacheable */
				rcr_vb ? 2 : 0,			/* RCR production notification mode */
				0,				/* DCM: CI write mode */
				eqcr_vb ? 2 : 0,		/* EQCR production notification mode */
				memback ? 1 : 0,		/* VPM: VDQCR production notification mode */
				memback ? 1 : 0,		/* CPM: CR production notification mode */
				0,				/* mem stashing drop enable */
				0,				/* mem stashing priority */
				0,				/* mem stashing enable */
				de ? 1 : 0,			/* dequeue stashing priority */
				de ? 1 : 0,			/* dequeue stashing enable */
				est ? 1 : 0);			/* EQCR_CI stashing priority */
	qbman_cinh_write(s, SWP_CINH_CFG, reg);

	if (!memback && __raw_readb(qbman_cinh_base(s, SWP_CINH_RR(1)))) {
		s->cr.valid_bit = 0;
	} else {
		s->cr.valid_bit = QB_VALID_BIT;
	}

	reg = qbman_cinh_read(s, SWP_CINH_DQRR_PI);
	s->dqrr.valid_bit = reg & QB_VALID_BIT ? QB_VALID_BIT : 0;
	s->dqrr.pi = reg & s->dqrr.pi_ci_mask;
	s->dqrr.ci = qbman_cinh_read(s, SWP_CINH_DQRR_CI) & s->dqrr.pi_ci_mask;
	s->dqrr.fi = s->dqrr.ci;
	if (s->dqrr.pi != s->dqrr.ci) {
		pr_err("qbman: DQRR ring is not zero, portal is not clean\n");
		goto err;
	}

	s->eqcr.pi_ci_mask = memback ? 0x3f : 0xf;
	reg = qbman_cinh_read(s, SWP_CINH_EQCR_PI);
	s->eqcr.valid_bit = reg & QB_VALID_BIT ? QB_VALID_BIT : 0;
	s->eqcr.pi = reg & s->eqcr.pi_ci_mask;
	s->eqcr.ci = qbman_cinh_read(s, SWP_CINH_EQCR_CI) & s->eqcr.pi_ci_mask;
	s->eqcr.fi = s->eqcr.pi;
	if (s->eqcr.pi != s->eqcr.ci) {
		pr_err("qbman: EQCR ring is not zero, portal is not clean\n");
		goto err;
	}

	s->rcr.pi_ci_mask = 0x3;
	reg = qbman_cinh_read(s, SWP_CINH_RCR_PI);
	s->rcr.valid_bit = reg & QB_VALID_BIT ? QB_VALID_BIT : 0;
	s->rcr.pi = reg & s->rcr.pi_ci_mask;
	s->rcr.ci = qbman_cinh_read(s, SWP_CINH_RCR_CI) & s->rcr.pi_ci_mask;
	s->rcr.fi = s->rcr.pi;
	if (s->rcr.pi != s->rcr.ci) {
		pr_err("qbman: RCR ring is not zero, portal is not clean\n");
		goto err;
	}

	if (s->desc->flags & SWP_F_MEMBACK) {
		memset(d->cena_bar, 0, d->cena_size);
	} else {
		dma_rmb();
		n = d->cena_size >> L1_CACHE_SHIFT;
		for (i = 0; i < n; i++) {
			dcivac(d->cena_bar + (i << L1_CACHE_SHIFT));
		}
	}

	return s;
err:
	if (s) {
		MTRACE_KMEM_DEL(s);
		kfree(s);
	}
	return NULL;
}

void qbman_swp_finish(struct qbman_swp *s)
{
	qbman_cinh_write(s, SWP_CINH_SDQCR, 0);
	qbman_cinh_write(s, SWP_CINH_IER, 0);
	qbman_cinh_write(s, SWP_CINH_ISDR, 0xffffffff);
	qbman_cinh_write(s, SWP_CINH_IIR, 1);
	qbman_cinh_write(s, SWP_CINH_ISR, 0xffffffff);

	MTRACE_KMEM_DEL(s);
	kfree(s);
}

void __iomem *qbman_swp_cr_next(struct qbman_swp *s, u8 *cmd_verb)
{
	void __iomem *p;

	if (s->desc->flags & SWP_F_MEMBACK) {
		p = qbman_cena_base(s, SWP_CENA_CR_MEM);
	} else {
		p = qbman_cena_base(s, SWP_CENA_CR);
	}
	*cmd_verb = s->cr.valid_bit;

	return p;
}

const void __iomem *qbman_swp_rr_next(struct qbman_swp *s, u8 cmd_verb)
{
	const void __iomem *p;

	if (s->desc->flags & SWP_F_MEMBACK) {
		p = qbman_cena_base(s, SWP_CENA_RR_MEM);
		if (__raw_readb(p) != cmd_verb) {
			return NULL;
		}
	} else {
		p = qbman_cena_base(s, SWP_CENA_RR(s->cr.valid_bit ? 1 : 0));
		if (__raw_readb(p) != cmd_verb) {
			dma_rmb();
			dcivac(p);
			prefetch(p);
			return NULL;
		}
	}

	s->cr.valid_bit ^= QB_VALID_BIT;
	return p;
}

void __iomem *qbman_swp_eqcr_next(struct qbman_swp *s, u8 *cmd_verb)
{
	u8 free, half_mask;
	unsigned long flags;

	flags = s->desc->flags;
	half_mask = s->eqcr.pi_ci_mask >> 1;
	free = half_mask + 1 - qm_cyc_diff(s->eqcr.pi_ci_mask, s->eqcr.ci, s->eqcr.pi);
	if (!free) {
		if (flags & SWP_F_EST) {
			if (flags & SWP_F_MEMBACK) {
				s->eqcr.ci = qbman_cena_read(s, SWP_CENA_EQCR_CI_MEMBACK) & s->eqcr.pi_ci_mask;
			} else {
				s->eqcr.ci = qbman_cena_read(s, SWP_CENA_EQCR_CI) & s->eqcr.pi_ci_mask;
			}
		} else {
			s->eqcr.ci = qbman_cinh_read(s, SWP_CINH_EQCR_CI) & s->eqcr.pi_ci_mask;
		}
		free = half_mask + 1 - qm_cyc_diff(s->eqcr.pi_ci_mask, s->eqcr.ci, s->eqcr.pi);
	}

	if (free) {
		*cmd_verb = s->eqcr.valid_bit;
		return qbman_cena_base(s, SWP_CENA_EQCR(s->eqcr.pi & half_mask));
	} else {
		return NULL;
	}
}

void __iomem *qbman_swp_rcr_next(struct qbman_swp *s, u8 *cmd_verb)
{
	u8 free, half_mask;

	half_mask = s->rcr.pi_ci_mask >> 1;
	free = half_mask + 1 - qm_cyc_diff(s->rcr.pi_ci_mask, s->rcr.ci, s->rcr.pi);
	if (!free) {
		s->rcr.ci = qbman_cinh_read(s, SWP_CINH_RCR_CI) & s->rcr.pi_ci_mask;
		free = half_mask + 1 - qm_cyc_diff(s->rcr.pi_ci_mask, s->rcr.ci, s->rcr.pi);
	}

	if (free) {
		*cmd_verb = s->rcr.valid_bit;
		if (s->desc->flags & SWP_F_MEMBACK) {
			return qbman_cena_base(s, SWP_CENA_RCR_MEM(s->rcr.pi & half_mask));
		} else {
			return qbman_cena_base(s, SWP_CENA_RCR(s->rcr.pi & half_mask));
		}
	} else {
		return NULL;
	}
}

const void __iomem *qbman_swp_dqrr_next(struct qbman_swp *s)
{
	u8 n, avail, half_mask;
	void __iomem *p;
	unsigned long flags;

	flags = s->desc->flags;
	half_mask = s->dqrr.pi_ci_mask >> 1;
	avail = qm_cyc_diff(s->dqrr.pi_ci_mask, s->dqrr.ci, s->dqrr.pi);
	if (!avail) {
		if (flags & SWP_F_DQRR_VB) {
			if (flags & SWP_F_MEMBACK) {
				p = qbman_cena_base(s, SWP_CENA_DQRR_MEM(s->dqrr.pi & half_mask));
			} else {
				p = qbman_cena_base(s, SWP_CENA_DQRR(s->dqrr.pi & half_mask));
			}
			if ((__raw_readb(p) & QB_VALID_BIT) == s->dqrr.valid_bit) {
				s->dqrr.pi = (s->dqrr.pi + 1) & s->dqrr.pi_ci_mask;
				if (!(s->dqrr.pi & half_mask)) {
					s->dqrr.valid_bit ^= QB_VALID_BIT;
				}
			}
		} else {
			s->dqrr.pi = qbman_cinh_read(s, SWP_CINH_DQRR_PI) & s->dqrr.pi_ci_mask;
		}
		avail = qm_cyc_diff(s->dqrr.pi_ci_mask, s->dqrr.ci, s->dqrr.pi);
	}

	if (!(flags & SWP_F_DE) && (flags & SWP_F_DQRR_VB)) {
		n = qm_cyc_diff(s->dqrr.pi_ci_mask, s->dqrr.fi, s->dqrr.pi);
		if (n <= half_mask) {
			if (flags & SWP_F_MEMBACK) {
				p = qbman_cena_base(s, SWP_CENA_DQRR_MEM(s->dqrr.pi & half_mask));
			} else {
				p = qbman_cena_base(s, SWP_CENA_DQRR(s->dqrr.pi & half_mask));
			}
			dma_rmb();
			dcivac(p);
			prefetch(p);
		}
	}

	if (!avail) {
		return NULL;
	}

	if (flags & SWP_F_MEMBACK) {
		p = qbman_cena_base(s, SWP_CENA_DQRR_MEM(s->dqrr.ci & half_mask));
	} else {
		p = qbman_cena_base(s, SWP_CENA_DQRR(s->dqrr.ci & half_mask));
	}
	if (!(flags & SWP_F_DQRR_VB)) {
		dma_rmb();
		dcivac(p);
		prefetch(p);
	}
	return p;
}

void qbman_swp_cr_submit(struct qbman_swp *s, void __iomem *p, u8 cmd_verb)
{
	if (s->desc->flags & SWP_F_MEMBACK) {
		__raw_writeb(cmd_verb, p);
		dma_wmb();
		qbman_cinh_write(s, SWP_CINH_CR_RT, QB_RT_BIT);
	} else {
		dma_wmb();
		__raw_writeb(cmd_verb, p);
		dccvac(p);
		p = qbman_cena_base(s, SWP_CENA_RR(s->cr.valid_bit ? 1 : 0));
		dma_rmb();
		dcivac(p);
		prefetch(p);
	}
}

int qbman_swp_eqcr_submit(struct qbman_swp *s, void __iomem *p, u8 cmd_verb, bool flush)
{
	u8 i, n, half_mask;
	unsigned long flags;

	flags = s->desc->flags;
	half_mask = s->eqcr.pi_ci_mask >> 1;

	if (p) {
		i = SWP_CENA_EQCR_IDX(p - qbman_cena_base(s, 0));
		if (i != (s->eqcr.pi & half_mask)) {
			return -EINVAL;
		}
		if (flags & SWP_F_EQCR_VB) {
			dma_wmb();
		}
		__raw_writeb(cmd_verb, p);
		if (!(flags & SWP_F_MEMBACK)) {
			dccvac(p);
		}
		s->eqcr.pi = (s->eqcr.pi + 1) & s->eqcr.pi_ci_mask;
		if ((flags & SWP_F_EQCR_VB) && !(s->eqcr.pi & half_mask)) {
			s->eqcr.valid_bit ^= QB_VALID_BIT;
		}
	}

	if (!(flags & SWP_F_EQCR_VB)) {
		n = qm_cyc_diff(s->eqcr.pi_ci_mask, s->eqcr.fi, s->eqcr.pi);
		if (n && ((n >= s->eqcr.pi_ci_mask >> 3) || flush)) {
			dma_wmb();
			if (flags & SWP_F_MEMBACK) {
				qbman_cinh_write(s, SWP_CINH_EQCR_PI, QB_RT_BIT | s->eqcr.pi);
			} else {
				qbman_cinh_write(s, SWP_CINH_EQCR_PI, s->eqcr.pi);
			}
			s->eqcr.fi = s->eqcr.pi;
		}
	}

	return 0;
}

int qbman_swp_rcr_submit(struct qbman_swp *s, void __iomem *p, u8 cmd_verb, bool flush)
{
	u8 i, n, half_mask;
	unsigned long flags;

	flags = s->desc->flags;
	half_mask = s->rcr.pi_ci_mask >> 1;

	if (p) {
		if (flags & SWP_F_MEMBACK) {
			i = SWP_CENA_RCR_MEM_IDX(p - qbman_cena_base(s, 0));
		} else {
			i = SWP_CENA_RCR_IDX(p - qbman_cena_base(s, 0));
		}
		if (i != (s->rcr.pi & half_mask)) {
			return -EINVAL;
		}
		if (flags & SWP_F_RCR_VB) {
			dma_wmb();
		}
		__raw_writeb(cmd_verb, p);
		if (!(flags & SWP_F_MEMBACK)) {
			dccvac(p);
		}
		s->rcr.pi = (s->rcr.pi + 1) & s->rcr.pi_ci_mask;
		if ((flags & SWP_F_RCR_VB) && !(s->rcr.pi & half_mask)) {
			s->rcr.valid_bit ^= QB_VALID_BIT;
		}
	}

	if (!(flags & SWP_F_RCR_VB)) {
		n = qm_cyc_diff(s->rcr.pi_ci_mask, s->rcr.fi, s->rcr.pi);
		if (n && ((n >= s->rcr.pi_ci_mask >> 3) || flush)) {
			dma_wmb();
			if (flags & SWP_F_MEMBACK) {
				qbman_cinh_write(s, SWP_CINH_RCR_PI, QB_RT_BIT | s->rcr.pi);
			} else {
				qbman_cinh_write(s, SWP_CINH_RCR_PI, s->rcr.pi);
			}
			s->rcr.fi = s->rcr.pi;
		}
	}

	return 0;
}

int qbman_swp_dqrr_consume(struct qbman_swp *s, const void __iomem *p, bool flush)
{
	u8 i, n, half_mask;

	half_mask = s->dqrr.pi_ci_mask >> 1;

	if (p) {
		if (s->desc->flags & SWP_F_MEMBACK) {
			i = SWP_CENA_DQRR_MEM_IDX(p - qbman_cena_base(s, 0));
		} else {
			i = SWP_CENA_DQRR_IDX(p - qbman_cena_base(s, 0));
		}
		if (i != (s->dqrr.ci & half_mask)) {
			return -EINVAL;
		}
		s->dqrr.ci = (s->dqrr.ci + 1) & s->dqrr.pi_ci_mask;
	}

	n = qm_cyc_diff(s->dqrr.pi_ci_mask, s->dqrr.fi, s->dqrr.ci);
	if (n && ((n >= s->dqrr.pi_ci_mask >> 3) || flush)) {
		qbman_cinh_write(s, SWP_CINH_DQRR_CI, s->dqrr.ci);
		s->dqrr.fi = s->dqrr.ci;
	}

	return 0;
}

void qbman_swp_push_get(struct qbman_swp *s, u8 idx, bool *en)
{
	u16 src = (s->sdq >> SWP_SDQCR_SRC_SHIFT) & SWP_SDQCR_SRC_MASK;

	WARN_ON(idx > 15);
	*en = src & (1 << idx) ? true : false;
}

void qbman_swp_push_set(struct qbman_swp *s, u8 idx, bool en)
{
	u16 src;

	WARN_ON(idx > 15);
	if (en) {
		s->sdq |= 1 << idx;
	} else {
		s->sdq &= ~(1 << idx);
	}

	src = (s->sdq >> SWP_SDQCR_SRC_SHIFT) & SWP_SDQCR_SRC_MASK;
	if (src) {
		qbman_cinh_write(s, SWP_CINH_SDQCR, s->sdq);
	} else {
		qbman_cinh_write(s, SWP_CINH_SDQCR, 0);
	}
}
